package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.DaisyWristSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.DifferentialSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.DrivetrainSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.KittyNet;
import edu.wpi.first.wpilibj.templates.subsystems.ShooterSubsystem;

/**
 * The base for all commands. All atomic commands should subclass CommandBase.
 * CommandBase stores creates and stores each control system. To access a
 * subsystem elsewhere in your code in your code use CommandBase.exampleSubsystem
 * @author Author
 */
public abstract class CommandBase extends Command {

    public static OI oi;
    
    // Create a single static instance of all of your subsystems
    public static DrivetrainSubsystem leftDrivetrain =
            new DrivetrainSubsystem(
            "Left Drivetrain",
            RobotMap.SIDECAR_ONE,
            RobotMap.LEFT_FORE_VICTOR,
            RobotMap.LEFT_REAR_VICTOR,
            RobotMap.LEFT_ENCODER_A_CHANNEL,
            RobotMap.LEFT_ENCODER_B_CHANNEL,
            true);
    
    public static DrivetrainSubsystem rightDrivetrain = 
            new DrivetrainSubsystem(
            "Right Drivetrain",
            RobotMap.SIDECAR_ONE,
            RobotMap.RIGHT_FORE_VICTOR,
            RobotMap.RIGHT_REAR_VICTOR,
            RobotMap.RIGHT_ENCODER_A_CHANNEL,
            RobotMap.RIGHT_ENCODER_B_CHANNEL,
            false);
    
    public static DifferentialSubsystem gearShifters =
            new DifferentialSubsystem(
            "Differentials",
            RobotMap.SIDECAR_ONE,
            RobotMap.LEFT_DIFFERENTIAL_PORT,
            RobotMap.RIGHT_DIFFERENTIAL_PORT);
    
    public static ArmSubsystem mainArm = 
            new ArmSubsystem(
            "Main Arm",
            RobotMap.SIDECAR_TWO,
            RobotMap.LEFT_ARM_PWM_PORT,
            RobotMap.RIGHT_ARM_PWM_PORT,
            RobotMap.ARM_POT_PORT);
    
    public static DaisyWristSubsystem intake = 
            new DaisyWristSubsystem(
            "Daisy Intake",
            RobotMap.SIDECAR_TWO,
            RobotMap.LEFT_INTAKE_PWM_PORT,
            RobotMap.RIGHT_INTAKE_PWM_PORT,
            RobotMap.WRIST_PWM_PORT,
            RobotMap.WRIST_POT_PORT);
    
   public static ShooterSubsystem shooter =
            new ShooterSubsystem(
            "Shooter",
            RobotMap.SIDECAR_TWO,
            RobotMap.FLYWHEEL_PWM_PORT,
            RobotMap.TOP_INDEXER_PWM_PORT,
            RobotMap.BOTTOM_INDEXER_PWM_PORT,
            RobotMap.FIRST_STOP_PWM,
            RobotMap.FIRST_TOP_PORT,
            RobotMap.FIRST_BOTTOM_PORT,
            RobotMap.SECOND_TOP_PORT,
            RobotMap.SECOND_BOTTOM_PORT,
            RobotMap.THIRD_TOP_PORT,
            RobotMap.THIRD_BOTTOM_PORT);
            
    public static KittyNet kittyNet = 
            new KittyNet(
            "KittyNet");

    public static void init() {
        // This MUST be here. If the OI creates Commands (which it very likely
        // will), constructing it during the construction of CommandBase (from
        // which commands extend), subsystems are not guaranteed to be
        // yet. Thus, their requires() statements may grab null pointers. Bad
        // news. Don't move it.
        oi = new OI();

        // Show what command your subsystem is running on the SmartDashboard
       
        SmartDashboard.putData(leftDrivetrain);
        SmartDashboard.putData(rightDrivetrain);
        
    } // end init

    public CommandBase(String name) {
        super(name);
    } // end CommandBase Constructor with a name

    public CommandBase() {
        super();
    } // end CommandBase Default Constructor
    
} // end CommandBase
